/*
 * mahony.h
 *
 *  Created: 20250116
 *  Author: Hecate
 */

#ifndef CODE_MAHONY_H_
#define CODE_MAHONY_H_

// Mahony滤波以1 ms周期调用，角速度单位为弧度每秒

// 6轴IMU姿态解算数据
typedef struct
{
    float q[4];              // quaternion
    float *gx, *gy, *gz;     // gyroscope rates
    float *ax, *ay, *az;     // accelerometer readings
    float pitch, roll, yaw;  // Euler angles
} _6IMUDATA;

// 9轴IMU姿态解算数据
typedef struct
{
    float q[4];              // quaternion
    float *gx, *gy, *gz;     // gyroscope rates
    float *ax, *ay, *az;     // accelerometer readings
    float *mx, *my, *mz;     // magnetometer readings
    float pitch, roll, yaw;  // Euler angles
} _9IMUDATA;

// 初始化6轴Mahony滤波器
void mahony6Init(_6IMUDATA* obj, float* gx, float* gy, float* gz, float* ax, float* ay, float* az);

// 初始化9轴Mahony滤波器
void mahony9Init(_9IMUDATA* obj, float* gx, float* gy, float* gz, float* ax, float* ay, float* az, float* mx, float* my, float* mz);

// 更新6轴Mahony滤波器并更新姿态
void mahony6Update(_6IMUDATA* obj);

// 更新9轴Mahony滤波器并更新姿态
void mahony9Update(_9IMUDATA* obj);

#endif /* CODE_MAHONY_H_ */
